/*
 * Blobfinder.java
 *
 * Created on 17 de febrero de 2008, 12:48 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot.Sensors;

import Robot.*;
import javaclient2.*;
import java.util.ArrayList;
import javaclient2.structures.PlayerConstants;
import javaclient2.structures.blobfinder.*;
import javaclient2.BlobfinderInterface;
import Util.*;
import java.util.Vector;
/**
 *
 * @author alejo
 */
public class Blobfinder extends Sensor implements Vector2DArraySensor{
    
    BlobfinderInterface BFDev;
    int numIRs;
    
    
    Vector2d[] data;
    ColorBlob obstColor;
    
    PlayerBlobfinderData dataBF;
    
    PlayerBlobfinderBlob[] blobs;
    int blobsCount;
    
    BlobfinderFile bff;
    
    int minArea, maxArea;
    double[] transMatrix = new double[9];
    double[] intrinsicParams = new double[9];
    double[] radialDistParams = new double[2];
    
    /** Creates a new instance of IR */
    public Blobfinder(Robot robot, String name, int index) throws Exception{
        super(robot, name, index);
        BFDev=robot.getConnection().requestInterfaceBlobfinder(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);
        
        if(BFDev==null)
               throw new Exception("Error requesting interface Blobfinder!");
        else
            this.setPlayerDevice(BFDev);
    
        
        //TODO: Falta poner la opción de que si es null las matrices, no se utilicen
        bff = new BlobfinderFile("params.txt","/home/alejo/Escritorio/Tesis/Drivers/BFPosition");
        bff.createFile();
        obstColor = bff.getColorObst();
        minArea = bff.getMinArea();
        maxArea = bff.getMaxArea();
        transMatrix = bff.getProyectionMatrix();
        intrinsicParams = bff.getIntrinsicMatrix();
        radialDistParams = bff.getLensDistMatrix();
        data = new Vector2d[1];
        data[0] = new Vector2d(0,0,Vector2d.MODE_XY);
        
    }
    
     public Vector2d[] read(int mode) {
        
        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data; 
    }
     
    public boolean update(){
        
        if(isReady()) {
            
            //get data from blobfinder
            dataBF = BFDev.getData();
            blobs = dataBF.getBlobs();
            blobsCount = dataBF.getBlobs_count();
            
            //buffer for vectors to obstacles
            //TODO: para pruebas se definirán todos los blobs como obstáculos, INCLUYENDO
            //los blobs que conforman a los robots.
            Vector dataTemp = new Vector(1,1);
            //data = new Vector2d[blobsCount];
            //System.out.println("Number of blobs: "+blobsCount);
            int dataCount=0;
            for(int i=0;i<blobsCount;i++)
            {
                if(blobs[i].getColor()==obstColor.getRGB() && blobs[i].getArea()>minArea && blobs[i].getArea()<maxArea )
                {
                    int x = blobs[i].getX();
                    int y = blobs[i].getY();

                    Vector2d blobPosPixel = new Vector2d(x,y,Vector2d.MODE_XY);
                    Vector2d blobPosProy = new Vector2d();
                    //aqui irian funciones que convierten la x y la y a las coordenadas en las que está 
                    //referenciados los robots.
                    //proyectPoints(blobPose, blobPose ); //el primero está sin proyección, el segundo con

                    correctLensDist(blobPosPixel,blobPosProy);
                    proyectPoints(blobPosPixel,blobPosProy);

                    
                    //System.out.print("Vector No. " + dataCount+" ");
                    //System.out.println(blobPosProy.toString());
                    //data[dataCount++] = blobPosProy;
                    dataTemp.addElement(blobPosProy);
                    dataCount++;
                }
                else
                {
                   // System.out.println(blobs[i].getColor()+ " "+ blobs[i].getArea());
                
                }
                
            }
            
            //aqui ya se han calculado todas las posiciones de los obstáculos en el terreno. Estos calculos han sido realizados
            //en coordenadas globales!! 
            if(!dataTemp.isEmpty())
            {
                data = new Vector2d[dataTemp.size()];
                dataTemp.toArray(data);
            }
            
            //System.out.println("numero de datos: " +data.length);
            return true;
        }
        else
            return false;
    }
    
    public boolean isReady()
    {
        return BFDev.isDataReady();
    }
    
    public void loadProyectionMatrix()
    {
    
    }
    
    public void loadLensParams()
    {
    
    }
    
    private boolean correctLensDist(Vector2d src, Vector2d dst)
    {
        //get center of image
	double u0 = intrinsicParams[2];
	double v0 = intrinsicParams[5];
	//get f/dx and f/dy
	double fx = intrinsicParams[0];
	double fy = intrinsicParams[4];
	
	
	double x = src.getX() - u0;
	double y = src.getY() - v0;
	
	//now we have the coordinates related to the lens
	x/=fx;
	y/=fy;
	
	double r = x * x + y * y;  

	double radialX = x/(1 + radialDistParams[0]*r + radialDistParams[1]*r*r);
	
	dst.setX((float)(radialX*fx + u0));
	
	double radialY = y/(1 + radialDistParams[0]*r + radialDistParams[1]*r*r);
	
	dst.setY((float)(radialY*fy + v0));

	return true;
    }
    
    private void proyectPoints(Vector2d src, Vector2d dst)
    {
        double imgPoint[]={src.getX(),src.getY(),1};//Punto dado
        double[] realPoint = new double[3];

        realPoint[0] = transMatrix[0]*imgPoint[0] + transMatrix[1]*imgPoint[1] + transMatrix[2]*imgPoint[2];
        realPoint[1] = transMatrix[3]*imgPoint[0] + transMatrix[4]*imgPoint[1] + transMatrix[5]*imgPoint[2];
        realPoint[2] = transMatrix[6]*imgPoint[0] + transMatrix[7]*imgPoint[1] + imgPoint[2];



        dst.setX((float)(realPoint[0]/realPoint[2]));
        dst.setY((float)(realPoint[1]/realPoint[2]));
    }
    

    
}

